#include "Arrive.h"
#include "Agent.h"
#include "Vec3.h"
#include <cmath>
#include "SteeringOutput.h"

namespace Engine
{
	Arrive::Arrive():m_targetRadius(0.0f), 
					 m_targetRadiusSquared(0.0f), 
					 m_slowRadius(0.0f), 
					 m_slowRadiusSquared(0.0f),
					 m_timeToTarget(0.1f){}

	void Arrive::SetTargetRadius(float r) {m_targetRadius = r; m_targetRadiusSquared = std::pow(r, 2.0f);}
	
	void Arrive::SetSlowRadius(float r) {m_slowRadius = r; m_slowRadiusSquared = std::pow(r, 2.0f);}

	void Arrive::Update(SteeringOutput* output)
	{
		Vec3 direction = m_parent ->GetTarget() ->GetPos() - m_parent ->GetPos();

		float distanceSqr = direction.SquaredLength();

		if(distanceSqr < m_targetRadiusSquared)
			return;

		float targetSpeed = m_parent ->GetMaxSpeed();

		if(distanceSqr < m_slowRadiusSquared)
			targetSpeed = targetSpeed * direction.Length() / direction.Length();

		Vec3 targetVel = direction;
		targetVel.Normalize();
		targetVel *= targetSpeed;

		output ->linear = targetVel - m_parent ->GetVel();
		output ->linear /= m_timeToTarget;

		float maxAccSquared = m_parent ->GetMaxAcc();
		maxAccSquared = maxAccSquared * maxAccSquared;

		if(output ->linear.SquaredLength()  > maxAccSquared)
		{
			output ->linear.Normalize();
			output ->linear *= m_parent ->GetMaxAcc();
		}
	}
}